# File: task1_env0329.py (完整改进版)    F-16C实验
# 任务1：起飞阶段完成起飞后最快到达第一个航点
# 任务2：起飞阶段最快起飞
# 任务3：非配平状态下的增稳起飞控制

import numpy as np
import math
import time
import gym
from gym import spaces
from getDCSdata import get_dcs_data  # 从getDCSdata模块导入获取DCS数据的函数
# from keyboard_control import *  # 从keyboard_control模块导入所有函数，用于控制飞机
from vjoy_control_new import *
import matplotlib.pyplot as plt  # 用于可视化
import logging
import csv
from FlightDataLogger import FlightDataLogger



logger = logging.getLogger(__name__)  # 获取日志记录器


def dms_to_decimal(degrees, minutes, seconds):  # 度分秒转换为十进制度数
    """
    degrees, minutes, seconds  = 度  分   秒
    """
    # 符号处理
    is_negative = degrees < 0
    degrees = abs(degrees)

    # 转换为十进制度数
    decimal_degrees = degrees + minutes / 60 + seconds / 3600

    # 返回符号
    if is_negative:
        decimal_degrees = -decimal_degrees

    return decimal_degrees


# 目标航点坐标以及到达目标航点的速度
TARGET_LONG = dms_to_decimal(42, 27, 32)  # 经度  修改为第一个航点的经纬度即可
TARGET_LAT = dms_to_decimal(41, 59, 49)  # 纬度
TARGET_ALT = 1500.00   # 高度
TARGET_VA = 150  # 速度   200 m/s == 720 km/h

# 初始位置（科布列季机场跑道）高度
INIT_ALT = 18  # 需要进行更改
# 飞机在跑道上的真实航向
INIT_HEADING = 64  # 需要进行更改

# 最小起飞速度
MIN_TAKEOFF_SPEED = 70


# CSV文件路径，用于记录训练日志
CSV_FILE_PATH = "results//training_log.csv"

# 配置日志
logging.basicConfig(
    level=logging.INFO,
    format='%(asctime)s - %(name)s - %(levelname)s - %(message)s',
    handlers=[
        logging.FileHandler("results//training.log"),
        logging.StreamHandler()
    ]
)


class TakeoffEnv(gym.Env):
    def __init__(self):

        try:
            activate_dcs_window()  # 窗口激活只在初始化时激活一次
        except Exception as e:
            logger.warning(f"窗口激活失败: {e}")

        # 动作空间：连续动作空间，包含6个维度，分别对应油门、俯仰和滚转控制
        #self.action_space = spaces.Box(low=-1.0, high=1.0, shape=(6,), dtype=np.float32)
        self.action_space = spaces.Box(
            low=np.array([0.0, -1.0, -1.0, -1.0, -1.0, -1.0]),  # 下限（长度6）
            high=np.array([1.0, 1.0, 1.0, 1.0, 1.0, 1.0]),  # 上限（长度6）
            shape=(6,),  # 明确形状
            dtype=np.float32
        )
        # 观察空间：包含飞机的各种状态信息，共XX个维度，包括速度、姿态、位置等   ----37个维度
        self.observation_space = spaces.Box(low=-np.inf, high=np.inf, shape=(37,), dtype=np.float32)

        self.last_distance = None  # 上一时刻到目标航点的距离
        self.last_altitude = None  # 上一时刻的高度

        self.steps = 0  # 当前步数
        self.current_episode = 0  # 当前回合数
        self.total_reward = 0  # 用于记录每个回合的总奖励
        self.is_on_ground = True  # 是否在地面上
        self.action = None
        self.last_pitch_action = 0.0
        self.last_pitch = 0.0
        self.last_bank = 0.0

        self.smoothed_action = [0] * 6  # 初始化平滑动作
        self.last_action = [0] * 6


        # 初始化CSV文件，写入表头
        self._init_csv_file()

        # 将当前坐标与目标坐标之间绘制直线
        self.reference_path = None  # 存储参考路径的经纬度点 全局参考路径
        self.path_generated = False  # 标志是否已生成路径
        self.flight_trajectory = []  # 记录飞行轨迹

        # 新增：记录飞行数据
        self.flight_data = []  # 存储飞行数据
        self.initial_position = None  # 初始位置
        self.target_position = (TARGET_LONG, TARGET_LAT, TARGET_ALT)  # 目标位置

        # 新增：航线规划相关
        self.waypoints = []  # 存储航线中间点 (经度, 纬度, 高度)
        self.current_waypoint_idx = 1  # 当前目标航点索引  从第二个航点开始
        self.reference_path = None  # 存储参考路径的经纬度点
        self.path_generated = False  # 标志是否已生成路径
        self.i = 1

        super().__init__()
        self.data_logger = FlightDataLogger()  # 初始化数据记录器
        self.current_reward_data = None  # 新增：用于临时存储奖励数据

    def _init_csv_file(self):
        """初始化回合数与奖励的CSV文件，写入表头"""
        with open(CSV_FILE_PATH, mode='w', newline='') as file:
            writer = csv.writer(file)
            writer.writerow(["Episode", "Step", "Reward", "Total Reward","ALT","IAS",])

    # def _generate_reference_path(self, start_long, start_lat, end_long=TARGET_LONG, end_lat=TARGET_LAT, num_points=11):
    #     """生成从起点到目标的直线路径（经纬度插值）"""
    #     # 如果没有中间检查点，直接生成起点到目标点的直线路径
    #     if not self.waypoints:
    #         longitudes = np.linspace(start_long, end_long, num_points)  # 起点 10个检查点  终点   一共需要11条小路径
    #         latitudes = np.linspace(start_lat, end_lat, num_points)
    #         self.reference_path = list(zip(longitudes, latitudes))
    #     else:
    #         # 如果有中间检查点，生成起点到检查点，再到目标点的路径
    #         self.reference_path = []
    #         # 起点到第一个检查点
    #         if self.waypoints:
    #             first_waypoint = self.waypoints[0]
    #             longitudes = np.linspace(start_long, first_waypoint[0], num_points // 2)
    #             latitudes = np.linspace(start_lat, first_waypoint[1], num_points // 2)
    #             self.reference_path.extend(list(zip(longitudes, latitudes)))
    #
    #         # 检查点之间
    #         for i in range(len(self.waypoints) - 1):
    #             current_waypoint = self.waypoints[i]
    #             next_waypoint = self.waypoints[i + 1]
    #             longitudes = np.linspace(current_waypoint[0], next_waypoint[0], num_points // len(self.waypoints))
    #             latitudes = np.linspace(current_waypoint[1], next_waypoint[1], num_points // len(self.waypoints))
    #             self.reference_path.extend(list(zip(longitudes, latitudes)))
    #
    #         # 最后一个检查点到目标点
    #         if self.waypoints:
    #             last_waypoint = self.waypoints[-1]
    #             longitudes = np.linspace(last_waypoint[0], end_long, num_points // 2)
    #             latitudes = np.linspace(last_waypoint[1], end_lat, num_points // 2)
    #             self.reference_path.extend(list(zip(longitudes, latitudes)))
    #
    #     self.path_generated = True
    #     logger.info(f"Generated reference path with {len(self.reference_path)} points")
    # def _generate_reference_path(self, start_long, start_lat, end_long=TARGET_LONG, end_lat=TARGET_LAT, num_points=100):
    #     """生成从起点到目标的直线路径（经纬度插值）"""
    #     longitudes = np.linspace(start_long, end_long, num_points)   # 经度
    #     latitudes = np.linspace(start_lat, end_lat, num_points)    # 纬度
    #     self.reference_path = list(zip(longitudes, latitudes))  # 存储参考路径的经度纬度
    #     self.path_generated = True  # 标志是否已生成路径
    #
    #     logger.info(f"Generated reference path with {num_points} points from ({start_long}, {start_lat}) to ({end_long}, {end_lat})")

    # def _write_reference_path(self):
    #     """将参考路径写入 reference_path.csv 文件"""
    #     csv_filename = "results//reference_path.csv"
    #     with open(csv_filename, 'w', newline='') as csvfile:
    #         writer = csv.writer(csvfile)
    #         writer.writerow(["Long", "Lat", "ALT"])  # 写入表头
    #         for point in self.reference_path:
    #             writer.writerow([point[0], point[1]])
    #     logger.info(f"Reference path written to {csv_filename}")

    def _plot_trajectory(self):
        """绘制飞行轨迹与参考路径对比"""
        if not self.reference_path or not self.flight_trajectory:
            return

        ref_longs, ref_lats = zip(*self.reference_path)
        traj_longs, traj_lats, _ = zip(*self.flight_trajectory)  # 使用飞行轨迹的经纬度

        plt.figure(figsize=(10, 6))
        plt.plot(ref_longs, ref_lats, 'b-', label='Reference Path')  # 蓝线表示参考路径
        plt.plot(traj_longs, traj_lats, 'g--', label='Flight Trajectory')  # 绿线表示飞行轨迹
        plt.scatter([TARGET_LONG], [TARGET_LAT], color='red', label='Target')  # 红点表示目标点
        plt.xlabel('Longitude')
        plt.ylabel('Latitude')
        plt.title('Flight Trajectory vs Reference Path')
        plt.legend()
        plt.grid(True)
        plt.savefig(f'trajectory_episode_{self.current_episode}.png')
        plt.close()
        logger.info(f"Flight trajectory saved as 'trajectory_episode_{self.current_episode}.png'")

    def reset(self):
        self.current_episode += 1
        logger.info(f"Resetting environment (Episode {self.current_episode})")

        # 激活DCS窗口  重置环境不需要激活窗口
        # activate_dcs_window()
        data = get_dcs_data()
        """
        data 是原始数据，仅包含直接从DCS获取的字段（如速度、姿态、位置等）。
        state 是增强数据，包含：
        原始数据（如 data["IAS"]）
        计算出的派生数据（如 delta_long = 当前经度 - 目标经度）
        控制输入（如 current_throttle 从vJoy读取）
        二者需通过 _extract_state() 明确转换，不可混用！
        """

        if data is None:
            logger.warning("Using fallback data after UDP timeout")
            data = self._get_fallback_data()  # 如果获取数据失败，使用模拟数据

        # 模拟非配平状态：随机初始角速度扰动
        data["Omega"] = [
            np.random.uniform(-0.0, 0.0),  # 滚转角速度 (rad/s)
            np.random.uniform(-0.0, 0.0),  # 俯仰角速度 (rad/s)
            np.random.uniform(-0.0, 0.0)  # 偏航角速度 (rad/s)
        ]
        logger.info(f"Non-trimmed state applied: Omega = {data['Omega']}")

        self.state = self._extract_state(data)  # 提取当前状态
        self.last_distance = self._calculate_distance(data)  # 计算到目标航点的距离
        self.last_altitude = data["LLA_balt"]  # 记录当前高度
        self.steps = 0
        self.total_reward = 0  # 重置总奖励

        # 检测飞机状态
        if  data["LLA_balt"] - INIT_ALT < 50.0:  # 速度<1m/s且高度<10m视为在地面   1    10    ----game    0          431    100 m/s == 360 km/H  data["IAS"] < 100.0 and
            self.is_on_ground = True
            logger.info("Aircraft detected on ground")
        else:
            self.is_on_ground = False
            logger.info("Aircraft detected in air")

        # 生成全局参考路径（任务 1）
        # if not self.path_generated:
        #     self._generate_reference_path(data["LLA_long"], data["LLA_lat"])
        #     self._write_reference_path()  # 写入参考路径到 CSV

        self.initial_position = (data["LLA_long"], data["LLA_lat"], data["LLA_balt"])
        self._generate_waypoints(data["LLA_long"], data["LLA_lat"], data["LLA_balt"])  # # 生成中间检查点
        self._write_waypoints()  # 写入中间检查点到 CSV

        # 修改初始航点索引为 1（第二个检查点）
        if len(self.waypoints) > 1:
            self.current_waypoint_idx = 1  # 从第二个检查点开始
        else:
            self.current_waypoint_idx = 0  # 如果只有一个检查点，从第一个开始

        self.flight_trajectory = []  # 重置飞行轨迹
        self.flight_data = []  # 重置飞行数据

        # 生成优化航线（任务 1 改进）



        # self.current_waypoint_idx = 0  # 从第一个航点开始

        # 写入初始数据到 CSV
        self._write_flight_data(data)

        self.action =[]

        return self.state

    def _generate_waypoints(self, start_long, start_lat, start_alt, num_points=10):
        """生成从起点到目标的优化航线，包含经纬度和高度"""
        if self.i < 2:
            end_long, end_lat, end_alt = TARGET_LONG, TARGET_LAT, TARGET_ALT
            num_points = 10
            longitudes = np.linspace(start_long, end_long, num_points)  #
            latitudes = np.linspace(start_lat, end_lat, num_points)  #
            altitudes = np.linspace(start_alt, end_alt, num_points)  # 平滑爬升
            self.waypoints = list(zip(longitudes, latitudes, altitudes))
            logger.info(f"Generated waypoints: {self.waypoints}")
            logger.info(f"Generated {num_points} waypoints: {self.waypoints}")
            self.i = self.i + 1

    def _write_waypoints(self):
        """将中间检查点写入 waypoints.csv 文件"""
        csv_filename = "results//waypoints.csv"
        with open(csv_filename, 'w', newline='') as csvfile:
            writer = csv.writer(csvfile)
            writer.writerow(["Long", "Lat", "Alt"])  # 写入表头
            for waypoint in self.waypoints:
                writer.writerow([waypoint[0], waypoint[1], waypoint[2]])
        logger.info(f"Waypoints written to {csv_filename}")

    def _get_fallback_data(self):
        """当UDP数据不可用时返回模拟数据"""
        return {
            "M_": 0.5, "IAS": 150, "heading_deg": 0,
            "pitch_deg": 5, "bank_deg": 0,
            "Point": [0, 100, 0], "VV": [100, 10, 0],
            "AoA": 5, "ACC": [0, 0, 0],
            "Fuel": 0.8, "Gears": 0, "Flaps": 0,
            "AirBrake": 0, "RPM": [90, 90, 0, 0, 0, 0],
            "LLA_long": 45.019, "LLA_lat": 41.636, "LLA_balt": 200,
            "Omega": [0.0, 0.0, 0.0]  # 角速度
        }

    def step(self, action):
        # 激活DCS窗口
        #activate_dcs_window()   # 每步调用激活窗口导致无法切换
        self.action = action  # 保存当前动作

        self._take_action(action)  # 执行动作

        # 控制频率动态调整

        # speed = data["IAS"]
        speed = self.state[1]  # 获取当前速度
        sleep_time = 0.2 if speed > 50 else 0.1
        time.sleep(max(sleep_time, 0))
        data = get_dcs_data()
        if data is None:
            logger.warning("Using fallback data in step")
            data = self._get_fallback_data()

        new_state = self._extract_state(data)  # 提取新状态
        current_distance = self._calculate_distance(data)  # 计算当前到目标航点的距离
        current_altitude = new_state[32]  # 获取当前高度
        altitude_change = current_altitude - self.last_altitude  # 计算高度变化
        current_speed = new_state[1]  # 获取当前速度

        # self.flight_trajectory.append((data["LLA_long"], data["LLA_lat"]))

        # 记录飞行数据
        self._write_flight_data(data)

        # 更新飞行轨迹
        self.flight_trajectory.append((self.state[30], self.state[31], self.state[32]))

        reward = self._calculate_reward(new_state, current_distance, altitude_change)  # 计算奖励
        done = self._is_done(new_state, current_distance, current_altitude)  # 判断是否结束  返回值为Ture , 表示达到终止条件
        # if done:
        #     time.sleep(3)
        #     revive_control(duration=0.3)  # 飞机复活


        # 累积总奖励
        self.total_reward += reward

        # 将回合数、步数和奖励写入CSV
        self._write_to_csv(self.current_episode, self.steps, reward, self.total_reward,current_altitude,current_speed)

        self.state = new_state
        self.last_distance = current_distance
        self.last_altitude = current_altitude
        self.steps += 1

        logger.info(
            f"Alt={current_altitude:.1f}m | "
            f"Spd={self.state[1]:.1f}m/s | "
            f"Pitch={self.state[3]:.1f}° | "
            f"Reward={reward:.2f}"
        )

         # 记录飞行数据到FlightDataLogger.py
        # 使用current_reward_data传递给记录器
        if self.current_reward_data is not None:
            self.data_logger.log_data(
                self.current_episode,
                self.steps,
                new_state,
                self.current_reward_data
            )

        # 每隔一定步数或回合结束时保存数据
        if done or self.steps % 100 == 0:
            self.data_logger.save_to_csv()
            self.data_logger.plot_parameters()

        return new_state, reward, done, {}

    def _write_flight_data(self, data):
        """将飞行数据写入 CSV 文件"""
        # 计算到当前航点的距离
        if self.current_waypoint_idx < len(self.waypoints):
            target_long, target_lat, target_alt = self.waypoints[self.current_waypoint_idx]
            dist_to_waypoint = self._calculate_distance_to_point(data, target_long, target_lat)
        else:
            dist_to_waypoint = self._calculate_distance(data)

        # 组织飞行数据
        flight_record = {
            "initial_position": self.initial_position,
            "target_position": self.target_position,
            "current_position": (data["LLA_long"], data["LLA_lat"], data["LLA_balt"]),
            "current_speed": data["IAS"],
            "current_altitude": data["LLA_balt"],
            "distance_to_waypoint": dist_to_waypoint
        }

        # 写入 CSV 文件
        csv_filename = "results//flight_data.csv"
        import os
        file_exists = os.path.exists(csv_filename)

        with open(csv_filename, 'a', newline='') as csvfile:
            writer = csv.writer(csvfile)
            if not file_exists:
                # 写入表头
                writer.writerow([
                    "Initial Long", "Initial Lat", "Initial Alt",
                    "Target Long", "Target Lat", "Target Alt",
                    "Current Long", "Current Lat", "Current Alt",
                    "Current Speed", "Current Altitude", "Distance to Waypoint"
                ])
            # 写入数据
            writer.writerow([
                self.initial_position[0], self.initial_position[1], self.initial_position[2],
                self.target_position[0], self.target_position[1], self.target_position[2],
                data["LLA_long"], data["LLA_lat"], data["LLA_balt"],
                data["IAS"], data["LLA_balt"], dist_to_waypoint
            ])

        # 添加到飞行数据列表（用于绘图）
        self.flight_data.append(flight_record)

    def _write_to_csv(self, episode, step, reward, total_reward,ALT,IAS):
        """将回合数、步数、奖励和总奖励写入CSV文件"""
        try:
            with open(CSV_FILE_PATH, mode='a', newline='') as file:
                writer = csv.writer(file)
                writer.writerow([episode, step, reward, total_reward,ALT,IAS])
                logger.info(
                    f"Data written to CSV: Episode={episode}, Step={step}, Reward={reward}, Total Reward={total_reward}")
        except Exception as e:
            logger.error(f"Failed to write to CSV: {e}")

    def _extract_state(self, data):
        delta_long = math.radians(data["LLA_long"] - TARGET_LONG) * 6371000  # 计算经度偏差
        delta_lat = math.radians(data["LLA_lat"] - TARGET_LAT) * 6371000  # 计算纬度偏差
        delta_alt = data["LLA_balt"] - TARGET_ALT  # 计算高度偏差
        print("经度偏差:", delta_long,"纬度偏差:",delta_lat)

        # 计算目标航向角（从当前位置指向目标位置）
        lat1, lon1 = math.radians(data["LLA_lat"]), math.radians(data["LLA_long"])
        lat2, lon2 = math.radians(TARGET_LAT), math.radians(TARGET_LONG)
        dlon = lon2 - lon1
        y = math.sin(dlon) * math.cos(lat2)
        x = math.cos(lat1) * math.sin(lat2) - math.sin(lat1) * math.cos(lat2) * math.cos(dlon)
        target_heading = math.degrees(math.atan2(y, x)) % 360  # 目标航向（0-360度）
        controls = get_current_controls()
        self.current_throttle = controls['throttle']  # 当前油门值 [0,1] （直接使用原始值）
        print("当前油门值:", self.current_throttle)
        self.current_pitch = controls['pitch']  # 当前俯仰摇杆值 [-1,1]
        print("当前俯仰值:",  self.current_pitch)
        self.current_roll = controls['roll']  # 当前滚转摇杆值 [-1,1]
        print("当前翻滚值:",  self.current_roll)
        self.current_rudder = controls['rudder']  # 当前脚舵值 [-1,1]
        print("当前脚舵值:", self.current_rudder)

        # 计算到参考路径的偏差
        if self.reference_path:
            path_deviation = self._calculate_path_deviation(data["LLA_long"], data["LLA_lat"])
            print("参考路径的偏差:", path_deviation)
        else:
            path_deviation = 0.0  # 如果路径未生成，默认偏差为0

        # 增加角速度和加速度信息，提取并返回状态向量
        '''
        状态空间中参数含义以及索引
            M_ : 马赫数  0   IAS: 指示空速 1  heading_deg:航向  2  pitch_deg:俯仰 3 bank_deg: 滚转 4  Point:位置   5 6 7
            VV:对地速度(三维)  8 9 10    AoA: 攻角 11  ACC:加速度 12 13 14     Fuel: 燃油 15   Gears:机轮  16
            Flaps: 襟翼  17   AirBrake:减速板  18    RPM:引擎 19  delta_long:经度偏差 20   delta_lat:纬度偏差  21   delta_alt: 高度偏差  22
            is_on_ground:地面标志位  23  Omega: 角速度 24 25 26    target_heading:目标航向  27   path_deviation: 路径偏差  28 
            --还需新增：
            VV_0:对地速度（一维）29  LLA_long：当前经度 30  LLA_lat：当前纬度 31 LLA_balt：当前高度 32
            油门值 [0,1] 32,俯仰摇杆值 [-1,1] 33,滚转摇杆值 [-1,1] 34 ,脚舵值 [-1,1] 35 
        '''
        state = np.array([
            data["M_"], data["IAS"], data["heading_deg"],
            data["pitch_deg"], data["bank_deg"],
            *data["Point"], *data["VV"], data["AoA"],
            *data["ACC"], data["Fuel"], data["Gears"],
            data["Flaps"], data["AirBrake"], data["RPM"][0],
            delta_long, delta_lat, delta_alt,
            # 1.0 if self.is_on_ground else 0.0,  # 添加标志位
            True if self.is_on_ground else False,  # 添加标志位
            *data["Omega"], # 角速度
            target_heading, # 新增目标航向
            path_deviation, # 新增路径偏差
            data["VV_0"],data["LLA_long"],data["LLA_lat"],data["LLA_balt"],
            # 新增控制输入状态（需从vJoy设备读取）
            self.current_throttle,  # 当前油门值 [0,1] （直接使用原始值）
            self.current_pitch,  # 当前俯仰摇杆值 [-1,1]
            self.current_roll,  # 当前滚转摇杆值 [-1,1]
            self.current_rudder,  # 当前脚舵值 [-1,1]
        ], dtype=np.float32)

        # 数据维度大小信息提示
        assert len(state) == self.observation_space.shape[0], f"State length {len(state)} does not match observation space {self.observation_space.shape[0]}"
        return state


    def _calculate_path_deviation(self, current_long, current_lat):
        # 计算到参考路径的偏差
        ref_points = np.array(self.reference_path)
        distances = np.sqrt((ref_points[:, 0] - current_long) ** 2 + (ref_points[:, 1] - current_lat) ** 2) * 6371000
        return np.min(distances)
    def _calculate_distance(self, data):
        lat1, lon1 = math.radians(data["LLA_lat"]), math.radians(data["LLA_long"])  # 将经纬度转换为弧度
        lat2, lon2 = math.radians(TARGET_LAT), math.radians(TARGET_LONG)
        return 6371000 * math.acos(
            math.sin(lat1) * math.sin(lat2) +
            math.cos(lat1) * math.cos(lat2) * math.cos(lon1 - lon2)
        )  # 使用球面距离公式计算距离
    def _take_action(self, action):
        '''
        throttle_action, pitch_action, roll_action, flaps_action, airbrake_action, foot_action
        油门              俯仰           滚转           襟翼           扰流板            脚舵
        '''
        '''
            执行飞机的控制动作，根据当前状态和输入的动作值控制飞机的油门、俯仰、滚转、襟翼、扰流板和脚舵。
            参数:
                action: 一个包含6个元素的数组，表示连续动作空间的控制量，范围为 [-1, 1]。油门除外，范围为 [-1, 1]
                        - throttle_action (action[0]): 油门控制，>0 增加油门，<0 减少油门
                        - pitch_action (action[1]): 俯仰控制，>0 机头向下（俯仰减小），<0 机头向上（俯仰增大）
                        - roll_action (action[2]): 滚转控制，>0 向右滚转，<0 向左滚转
                        - flaps_action (action[3]): 襟翼控制，>0 打开襟翼，<0 关闭襟翼
                        - airbrake_action (action[4]): 扰流板控制，>0 打开扰流板，<0 关闭扰流板
                        - foot_action (action[5]): 脚舵控制，>0 向左偏转，<0 向右偏转
            '''
        assert len(action) == 6, f"动作维度应为6，实际为{len(action)}"
        throttle_action, pitch_action, roll_action, flaps_action, airbrake_action, foot_action = action
        # 引入指数移动平均（EMA）平滑动作值，减少控制的突变
        # for i in range(6):
        #     self.smoothed_action[i] = 0.7 * self.last_action[i] + 0.3 * action[i]
        #     self.last_action[i] = self.smoothed_action[i]
        # throttle_action, pitch_action, roll_action, flaps_action, airbrake_action, foot_action = self.smoothed_action
        print("action", action)
        data = get_dcs_data()

        # 计算当前目标航点的航向和误差
        if not self.is_on_ground and self.current_waypoint_idx < len(self.waypoints):
            target_long, target_lat, target_alt = self.waypoints[self.current_waypoint_idx]
            lat1, lon1 = math.radians(data["LLA_lat"]), math.radians(data["LLA_long"])
            lat2, lon2 = math.radians(target_lat), math.radians(target_long)
            dlon = lon2 - lon1
            y = math.sin(dlon) * math.cos(lat2)
            x = math.cos(lat1) * math.sin(lat2) - math.sin(lat1) * math.cos(lat2) * math.cos(dlon)
            target_heading = math.degrees(math.atan2(y, x)) % 360   # 计算目标航向
            # 计算航向偏差
            heading_error = math.degrees(math.atan2(math.sin(math.radians(target_heading - data["heading_deg"])),
                                                    math.cos(math.radians(target_heading - data["heading_deg"]))))
            if heading_error > 180:
                heading_error -= 360   # 将航向修正到[-180, 180]
            elif heading_error < -180:
                heading_error += 360
            print("当前航向误差为:",heading_error)
        else:
            target_heading = data["heading_deg"]  # 地面阶段或无航点时保持当前航向
            heading_error = 0

        if self.is_on_ground:
            # 地面跑道阶段
            print("地面控制阶段：")
            print("目前飞机状态：高度",data["LLA_balt"],"速度：",data["IAS"],"襟翼：",data["Flaps"],"起落架：",data["Gears"])
            min_takeoff_speed = MIN_TAKEOFF_SPEED # 最小起飞速度   F-16C 起飞速度约 120 节 (62 m/s)，这里稍降低以优化  60 m/s == 216 km/h
            # 油门控制：最大化加速
            if throttle_action > 0:
                throttle_up(10 * throttle_action)  # 增加油门响应时间，为了快速获得更大的速度  0.5
            logger.debug(f"当前油门动作: {throttle_action} (范围检查: {0 <= throttle_action <= 1})")

            # 脚舵校正跑道方向
            runway_heading = INIT_HEADING  # 假设跑道航向为 0°，可根据实际调整
            heading_error_ground = runway_heading - data["heading_deg"]
            # 增加条件：速度低于最小起飞速度时使用脚舵
            if data["IAS"] < min_takeoff_speed:
                print("地面跑道航向角偏转：", heading_error_ground)
                if abs(heading_error_ground) > 2:  # 航向角偏转大小
                    # foot_control = np.clip(heading_error_ground * 0.1, -1, 1)
                    foot_control = foot_action

                    if foot_control > 0:
                        foot_left(duration=0.2)   # 向左调整
                    else:
                        foot_right(duration=0.2)  # 向右调整

            # 襟翼控制：尽早打开以增加升力
            '''
                 如果速度大于20 m/s且襟翼位置大于0.5（表示襟翼处于着陆襟翼0.9），目标为展开襟翼到0.25  
                 由于0.9-->0.5的过程会持续一段时间，有时会执行两次关闭襟翼动作，导致襟翼为0而不是所需要的0.25，,故将阈值改为0.8，
            '''
            if data["IAS"] > 0 and data["Flaps"] > 0.8:  # 降低速度阈值至 5 m/s == 18 km/h  为了快速加速。处于着陆襟翼时加速较慢
                flaps_control(duration=0.2)

            # 俯仰控制：达到最小起飞速度立即拉起
            print("襟翼：",data["Flaps"])
            if data["IAS"] > min_takeoff_speed and 0 < data["Flaps"] < 0.5:
                pitch_control = np.clip(pitch_action, -1, 1)
                if data["pitch_deg"] < 20 :
                    print("当前俯仰角小于20度，具体为：",data["pitch_deg"])
                    pitch_down(duration=4)   # 操作杆后拉
                    time.sleep(2)  # 状态延迟两秒
                    gearsoff_control() # 收起起落架
                # if pitch_control > 0:
                #     if 20 < data["pitch_deg"] < 45:
                #         print("当前俯仰角位于[20,45)度，具体为：", data["pitch_deg"])
                #         pitch_down(duration=5 * abs(pitch_control))
                #     if data["pitch_deg"] > 45 :
                #         print("当前俯仰角大于45度，具体为：", data["pitch_deg"])
                #         pitch_up(duration=5 * abs(pitch_control))

            # 滚转控制：RL 直接输出，负责增稳
            if data["IAS"] > min_takeoff_speed - 10 :
                roll_control = np.clip(roll_action, -1, 1)
                print("当前飞机滚转度为：",data["bank_deg"])
                print("roll_action:",roll_action)
                print("roll_control:",roll_control)
                if data["bank_deg"] >0:
                # if roll_action > 0:
                    # roll_right(duration=0.2 * abs(roll_control))
                    roll_left(duration=0.2)
                else:
                    # roll_left(duration=0.2 * abs(roll_control))
                    roll_right(duration=0.2)

            # 切换到空中模式
            if data["LLA_balt"] - INIT_ALT > 50 :  # 飞机高度比机场高度大于50米时，认为在空中
                self.is_on_ground = False
                logger.info("Aircraft has taken off, switching to air mode")
        else:
            print("空中控制阶段：")
            print("目前飞机状态：高度", data["LLA_balt"], "速度：", data["IAS"], "襟翼：", data["Flaps"], "起落架：",data["Gears"])

            # smoothed_pitch = 0.7 * self.last_pitch_action + 0.3 * pitch_action
            # self.last_pitch_action = smoothed_pitch
            # pitch_action = smoothed_pitch

            # 空中阶段：优化速度、高度和路径

            d_alt = TARGET_ALT - data["LLA_balt"]  # 高度偏差
            d_v = TARGET_VA - data["IAS"]  # 速度偏差
            print("高度偏差：",d_alt,"速度偏差：",d_v,"航向偏差:",heading_error)

            # 油门控制：加速至目标速度

            if d_v > 0 and throttle_action > 0:     # 小于目标速度
                throttle_up(0.3 * throttle_action)  # 油门增加
                if data["RPM"][0] < 0.75:  # 如果引擎小于0.75 ， 油门增加
                    throttle_up(0.3)
            elif d_v < 0 and throttle_action < 0:   # 大于目标速度
                throttle_down(0.3 * abs(throttle_action))  # 油门减少
                if data["RPM"][0] < 0.75:
                    throttle_up(0.3)

            # 俯仰控制：快速调整高度
            '''
                降低高度偏差阈值：将高度控制的触发阈值从100米调整为50米，提升精度。
                引入垂直速度反馈：结合垂直速度动态调整俯仰角，使高度控制更平滑。
            '''
            target_vspeed = np.clip(d_alt / 5, -5, 5)  # 目标垂直速度  # 计算目标垂直速度，限制在 [-10, 10] m/s
            vspeed_error = target_vspeed - data["VV"][2]  # 垂直速度偏差
            pitch_adjust = np.clip(vspeed_error * 0.1, -1, 1)   # 根据垂直速度偏差调整俯仰
            if d_alt > 300 and pitch_adjust > 0:   # 高度低于目标300米以上，机头向下（爬升）  貌似有问题
                # pitch_down(duration=0.1 * pitch_adjust)
                pitch_down(duration=0.1 )  #
                # time.sleep(1)
                # pitch_up(duration=0.1)
            elif d_alt < -300 and pitch_adjust < 0:   # 高度高于目标300米以上，机头向上（下降）
                # pitch_up(duration=0.1 * abs(pitch_adjust))
                pitch_up(duration=0.1 )
                # time.sleep(1)
                # pitch_down(duration=0.1)
            # if d_alt > 100 and pitch_action > 0:  # 低于目标高度，爬升
            #     pitch_down(duration=0.3 * pitch_action)
            # elif d_alt < -100 and pitch_action < 0:  # 高于目标高度，下降
            #     pitch_up(duration=0.3 * abs(pitch_action))

            # if abs(pitch_action) > 0.1:  # 降低阈值，增加俯仰控制频率
            #     pitch_down(duration=0.3 * pitch_action) if pitch_action > 0 else pitch_up(duration=0.3 * abs(pitch_action))

            #  滚转控制：调整航向至目标航点
            roll_target = np.clip(heading_error * 0.5, -10, 10)  # 限制滚转角度
            if roll_target > 0 and roll_action > 0:
                # roll_right(duration=0.3 * roll_action)  # 向右滚转
                roll_right(duration=0.3)   # 在不使用roll_action可以获得更大滚转幅度
            elif roll_target < 0 and roll_action < 0:
                # roll_left(duration=0.3 * abs(roll_action))  # 向左滚转
                roll_left(duration=0.3)

            # 襟翼控制：增加升力以快速爬升
            if d_alt > 500 and d_v < -1 and flaps_action > 0:  # 高度偏差大且速度大于目标速度，打开襟翼，便于快速爬升
                flaps_control()     # 打开襟翼
            elif d_alt < 50 and flaps_action < 0:   # 接近目标高度，关闭襟翼
                flaps_control()  # 关闭襟翼
            """
            # 扰流板控制：减速以调整高度或速度
            if d_v < -10 and airbrake_action > 0:
                 # airbrake_control(duration=0.2 * airbrake_action)       速度太高 ， 打开扰流板
                airbrake_on_control()
            elif d_v > 10 and airbrake_action < 0:
                # airbrake_control(duration=0.2 * abs(airbrake_action))   # 速度过低 ，关闭扰流板
                airbrake_off_control()
            """

            # 脚舵控制：精细调整航向
            foot_control = np.clip(heading_error * 0.1, -1, 1)
            if foot_control > 0:
                foot_left(duration=0.2 * foot_control)
            else:
                foot_right(duration=0.2 * abs(foot_control))

            # 检查是否到达当前航点（仅在空中阶段且有航点时执行）
            if self.current_waypoint_idx   < len(self.waypoints): # 当前检查点序号+1
                target_long, target_lat, target_alt = self.waypoints[self.current_waypoint_idx]
                dist_to_waypoint = self._calculate_distance_to_point(data, target_long, target_lat)
                # if dist_to_waypoint < 500 and abs(d_alt) < 100:  # 距离 < 500m 且高度偏差 < 100m 满足此条件进行航点切换
                #     self.current_waypoint_idx += 1
                print("当前目标是第",self.current_waypoint_idx,"个检查点")
                # 引入航向判断 确保飞机在接近航点时航向已对准下一个航点，避免偏离路径
                heading_to_waypoint = math.degrees(math.atan2(target_lat - data["LLA_lat"], target_long - data["LLA_long"])) % 360
                heading_error = min(abs(heading_to_waypoint - data["heading_deg"]),360 - abs(heading_to_waypoint - data["heading_deg"]))
                if dist_to_waypoint < 500 and abs(d_alt) < 200 and heading_error < 30:  #  距离 < 500m 且高度偏差 < 200m 满足此条件进行航点切换
                    self.current_waypoint_idx += 1
                    # 切换后立即调整航向和俯仰  在航点切换后，立即调整飞机的航向和俯仰角以对准下一个航点
                    if self.current_waypoint_idx < len(self.waypoints):
                        next_long, next_lat, next_alt = self.waypoints[self.current_waypoint_idx]
                        heading_to_next = math.degrees(
                            math.atan2(next_lat - data["LLA_lat"], next_long - data["LLA_long"])) % 360
                        heading_error = heading_to_next - data["heading_deg"]
                        pitch_adjust = np.clip((next_alt - data["LLA_balt"]) / 500, -1, 1)
                        if pitch_adjust > 0:
                            pitch_down(duration=0.1 * pitch_adjust)   # 调整俯仰
                        else:
                            pitch_up(duration=0.1 * abs(pitch_adjust))
                        roll_target = np.clip(heading_error * 0.2, -5, 5)
                        if roll_target > 0:
                            roll_right(duration=0.2 * roll_target)  # 调整滚转
                        else:
                            roll_left(duration=0.2 * abs(roll_target))
                    logger.info(f"Reached waypoint {self.current_waypoint_idx - 1}, moving to next")

        # time.sleep(0.5) # 等待操作完成
        data = get_dcs_data() # 获取数据
        if data is None:
            logger.warning("No feedback data after action execution")
        else:
            logger.debug(f"Action feedback - Altitude: {data['LLA_balt']:.1f}m, Speed: {data['IAS']:.1f}m/s")

    def _calculate_distance_to_point(self, data, target_long, target_lat):
        """计算到指定点的距离"""
        lat1, lon1 = math.radians(data["LLA_lat"]), math.radians(data["LLA_long"])
        lat2, lon2 = math.radians(target_lat), math.radians(target_long)
        return 6371000 * math.acos(
            math.sin(lat1) * math.sin(lat2) +
            math.cos(lat1) * math.cos(lat2) * math.cos(lon1 - lon2)
        )

    # def _calculate_reward(self, state, current_distance, altitude_change, altitude_change_reward=None):
    def _calculate_reward(self, state, current_distance, altitude_change):
        """
        计算当前步骤的奖励值，根据飞机的状态和任务目标设计奖励函数。
        参数:
            state: 当前状态向量，包含飞机的速度、姿态、位置等信息（33维）
            current_distance: 当前到目标航点的距离（米）
            altitude_change: 当前高度变化（米）
        返回:
            reward: 当前步骤的奖励值，综合考虑速度、高度、距离、稳定性等因素
        """
        reward = 0
        heading_reward = 0 #航向奖励
        alt_reward = 0  # 高度奖励
        alt_error_reward = 0  # 高度偏差惩罚
        alt_error_reward = 0  # 高度偏差惩罚
        vspeed_reward = 0
        stability_reward = 0
        speed_reward = 0
        dist_reward = 0
        delta_alt = state[22]  # 高度偏差
        current_altitude = delta_alt + TARGET_ALT  # 当前绝对高度
        speed = state[1]  # 指示空速
        pitch = abs(state[3])  # 俯仰角绝对值
        bank = abs(state[4])  # 滚转角绝对值
        heading = state[2]  # 当前航向
        target_heading = state[27]  # 目标航向
        path_deviation = state[28]  # 路径偏差
        aoa = state[11]  # 攻角
        vspeed = state[10]  # 垂直速度
        ground_speed = state[29]  # 对地速度

        pitch_rate = state[25]  # Omega[1] 俯仰角速度
        roll_rate = state[24]  # Omega[0] 滚转角速度
        # 检查油门值是否合法

        if not 0.0 <= self.current_throttle <= 1.0:
            logger.error(f"非法油门值: {self.current_throttle}")
            return -1e6  # 返回大惩罚，而非 -inf

        reward_data = {
            'dist_reward': 0,
            'heading_reward': 0,
            'speed_reward': 0,
            'vspeed_reward': 0,
            'stability_reward': 0,
            'total_reward': 0
        }



        # 动态权重
        if self.is_on_ground:
            w_speed = 0.25  # 提高速度权重
            w_altitude = 0.3
            w_distance = 0.0
            w_stability = 0.35  # 稳定性
            w_heading = 0.0  # 地面阶段轻微关注航向
            w_vspeed = 0.1  # 新增垂直速度权重

        else:
            w_speed = 0.15
            w_altitude = 0.15
            w_distance = 0.35  # 提高距离权重
            w_stability = 0.2
            w_heading = 0.15
            w_vspeed = 0.00  # 新增垂直速度权重


        # 地面阶段奖励（任务 2 优化）
        if self.is_on_ground:
            # 1. 速度奖励：激励快速加速
            speed_reward = 0
            min_takeoff_speed = 60  # 最小起飞速度
            if speed > min_takeoff_speed or speed == min_takeoff_speed:
                speed_reward += 50
            else:
                speed_reward -= 100

            print("地面阶段速度奖励：", speed_reward)

            # 2. 起飞奖励：成功离地
            altitude_change_reward = 0
            if altitude_change > 30:  # 高度增加 > 30
                altitude_change_reward += 50
            elif altitude_change < 0:
                altitude_change_reward -= 100

            print("地面阶段高度持续增加奖励：", altitude_change_reward)

            # 3. 稳定性奖励
            stability_reward = 0
            # 俯仰角
            if 5 < pitch < 30:
                stability_reward += 50
            elif 30 < pitch <45:
                stability_reward += 30
            else:
                stability_reward -= 100
            # 滚转角
            if abs(bank) < 2:
                stability_reward += 50
            elif 2 < abs(bank) < 5:
                stability_reward += 30
            else:
                stability_reward -= 100
            # 攻角
            if 8 < aoa < 20:  # 合理攻角范围
                stability_reward += 100
            else:
                stability_reward -= 200

            print("地面阶段稳定性奖励：", stability_reward)

            # 4. 垂直速度奖励
            vspeed_reward = 0
            if vspeed > 0:
                vspeed_reward += 50  # 激励离地
            else:
                vspeed_reward -= 100

            print("地面阶段垂直速度奖励：", vspeed_reward)

            # 5. 时间惩罚：激励最快起飞
            time_reward = 0
            time_reward -= self.steps * 10.0  # 每步惩罚 10.0，强迫快速离地
            print("地面阶段时间惩罚：", time_reward)

            # 6. 加权总奖励
            reward = w_speed * speed_reward + w_altitude * altitude_change_reward + w_stability * stability_reward + w_vspeed * vspeed_reward + time_reward
            print("地面阶段加权奖励：", reward)


            '''
            # 1. 速度奖励：激励快速加速
            target_speed = 55  # 最小起飞速度
            speed_reward = 100 * (speed / target_speed) if speed < target_speed else 100  # 线性奖励
            ground_speed_reward = 50 * (ground_speed / target_speed) if ground_speed < target_speed else 50
            # reward += w_speed * speed_reward
            reward += w_speed * (speed_reward + ground_speed_reward)

            # 2. 起飞奖励：成功离地
            if current_altitude > 50:
                reward += w_altitude * 300  # 提高离地奖励
                reward += 500 / (self.steps + 1)  # 越快离地奖励越高

            # 3. 稳定性奖励
            stability_reward = 0
            if 5 < pitch < 15:
                stability_reward += 50
            if abs(bank) < 5:
                stability_reward += 50
            if abs(pitch_rate) < 0.1:
                stability_reward += 100  # 强奖励俯仰角速度稳定
            if abs(roll_rate) < 0.1:
                stability_reward += 100  # 强奖励滚转角速度稳定
            # 惩罚过大角速度
            if abs(pitch_rate) > 0.5:
                stability_reward -= 50 * abs(pitch_rate)
            if abs(roll_rate) > 0.5:
                stability_reward -= 50 * abs(roll_rate)
            if 5 < aoa < 15:  # 合理攻角范围
                stability_reward += 50
            reward += w_stability * stability_reward

            # 4. 航向奖励
            if abs(heading - 0) < 10:
                reward += w_heading * 20
            else:
                reward -= w_heading * 50  #大惩罚航向问题

            # 5. 垂直速度奖励
            if vspeed > 0 and current_altitude < 50:  # 起飞前正垂直速度
                reward += w_vspeed * vspeed * 10  # 激励离地
            elif vspeed < -5:  # 惩罚下降
                reward -= w_vspeed * abs(vspeed) * 10

            # 6. 时间惩罚：激励最快起飞
            reward -= self.steps * 2.0  # 每步惩罚 2.0，强迫快速离地
            '''
        # 空中阶段奖励（任务 1 改进：跟踪航线）
        if not self.is_on_ground:
            target_speed = TARGET_VA  # 目标速度
            speed_error = abs(speed - target_speed)  # 实际速度与目标速度的差值绝对值
            # speed_reward = 50 * (1 - speed_error / target_speed) if speed_error < target_speed else -speed_error * 0.5
            speed_reward = 20 * (1 - speed_error / target_speed) if speed_error < 10 else -speed_error * 10  # 接近目标速度奖励，否则惩罚

            reward += w_speed * speed_reward
            # ground_speed_reward = 25 * (ground_speed / target_speed) if ground_speed < target_speed else 25
            # reward += w_speed * (speed_reward + ground_speed_reward)

            # 当前航点的距离和高度奖励    （仅限空中）
            if self.current_waypoint_idx < len(self.waypoints):
                target_long, target_lat, target_alt = self.waypoints[self.current_waypoint_idx]  # 获取当前航点的经纬度以及高度
                dist_to_waypoint = self._calculate_distance_to_point(
                    {"LLA_lat": state[31], "LLA_long": state[30]}, target_long, target_lat)  # 到当前航点距离
                alt_error = abs(target_alt - current_altitude)  # 高度差

                # 距离奖励 ：接近航点和距离减少
                if self.last_distance is not None:
                    distance_reduction = self.last_distance - dist_to_waypoint  # 距离减少量
                    # distance_reduction =  path_deviation

                    dist_reward = distance_reduction * 200  # 每米距离进行奖励
                    # if dist_to_waypoint < 500:  # 接近航点进行奖励
                    #     dist_reward += 10 * (1 - dist_to_waypoint / 500)  # 接近航点大幅奖励

                    # 到达航点进行奖励
                    if dist_to_waypoint < 500 and abs(delta_alt) < 100:
                        reward += 500  # 到达航点奖励
                    reward += w_distance * dist_reward
                    reward -= w_distance * state[28] / 100  # 路径偏差惩罚，每100米减分
                self.last_distance = dist_to_waypoint  # 更新 last_distance 为当前航点距离

                # 高度奖励
                alt_reward = 10 * (1 - (alt_error / 500) ** 2) if alt_error < 500 else -alt_error * 10  # 与目标高度距离小于500，进行奖励，否则进行惩罚
                alt_error_reward = -20 * (1 - (alt_error / 500) ** 2) if abs(alt_error) > 1000 else abs(alt_error) * 10  # 高度偏差惩罚
                '''
                如果alt_error > 1000（高度偏差极大）：
                惩罚公式：-20 * (1 - (alt_error / 500) ** 2)
                含义：
                当偏差远大于500米时，(alt_error / 500) ^ 2的值很大，导致(1 - x) 为负。最终惩罚为 - 20 * (负值) = 正惩罚（例如，偏差 1000米时惩罚为 - 20 * (1 - 4) = 60）。
                惩罚随偏差呈二次增长，比线性更严厉。
                目的：针对极端高度偏差（如俯冲或爬升过快）施加额外惩罚。
                如果 alt_error ≤ 1000：
                惩罚公式：abs(alt_error) * 10
                含义：每米偏差惩罚 10 点（与 alt_reward 的惩罚分支一致）。
                作用：保持对小偏差的线性惩罚，避免过度奖励。
                '''
                # reward += w_altitude * alt_reward
                reward += w_altitude * (alt_reward + alt_error_reward)


                # 航向奖励
                heading_error = 0
                heading_error = min(abs(heading - target_heading), 360 - abs(heading - target_heading))  # 航向偏差
                if heading_error < 5:  # 航向偏差小于10进行奖励，否则进行惩罚
                    heading_reward += w_heading * 20 * (1 - heading_error / 5)
                else:
                    heading_reward -= w_heading * heading_error * 50  #  -= 惩罚
                    reward += heading_reward


                # 垂直速度奖励/惩罚
                if abs(vspeed) < 10:
                    reward += w_vspeed * 20  # 保持平稳爬升或下降
                elif abs(vspeed) > 15:
                    reward -= w_vspeed * abs(vspeed) * 30  # 惩罚过大垂直速度

                # 攻角稳定性  攻角对于飞机的升力有很重要作用
                stability_reward1 = 0
                if 5 < aoa < 10:
                    # reward += w_stability * 50
                    stability_reward1 += w_stability * 50
                    reward += stability_reward1
                elif aoa < 0 or aoa > 15:
                    # reward -= w_stability * abs(aoa - 10) * 20
                    stability_reward1 -= w_stability * abs(aoa - 10) * 20
                    reward += stability_reward1



            # 稳定性惩罚
            stability_penalty = 0
            if pitch > 15:  # 俯仰角>10  20
                stability_penalty += (pitch - 10) * 50
            if pitch < -10:
                stability_penalty += (abs(pitch) - 10) * 50
            if abs(bank) > 15:
                stability_penalty += (bank - 15) * 50
            reward -= w_stability * stability_penalty

            if abs(pitch) < 10:
                stability_reward += 20
            if abs(bank) < 10:
                stability_reward += 20
            reward += w_stability * stability_reward

            reward -= 10.0  # 时间惩罚

        # 坠毁惩罚
        if current_altitude < 100:
            reward -= 2000

        self.last_pitch = pitch
        self.last_bank = bank

        reward_data['dist_reward'] = dist_reward
        reward_data['heading_reward'] = heading_reward
        reward_data['speed_reward'] = speed_reward
        reward_data['vspeed_reward'] = vspeed_reward
        reward_data['stability_reward'] = stability_reward
        reward_data['total_reward'] = reward

        # 记录飞行数据到FlightDataLogger.py
        self.current_reward_data = reward_data

        return reward

    def _is_done(self, state, current_distance, current_altitude):
        # 新增安全监测条件
        # if current_altitude < 200:
        #     # logger.warning("CRASH DETECTED!")
        #     logger.warning("飞机坠毁!")
        #     return True
        if abs(state[3]) > 40 or abs(state[4]) > 40:  # 更大姿态容限  俯仰  滚转
            logger.warning("危险姿态!")
            return True
        # if state[15] <= 0.05:
        #     logger.warning("Fuel almost exhausted")
        #     return True
        # if self.steps >= 100:
        #     logger.warning("Training time limit reached")
        #     return True
        # if not self.is_on_ground and current_distance < 500:
        #     logger.info("Target reached!")
        #     self._plot_trajectory()  # 到达目标时绘制轨迹
        #     return True
        if self.total_reward > 1000000:
            logger.info("奖励已达到最大！")
            return  True
        # if not self.is_on_ground and state[28] > 10000:  # 路径偏差超过 8km
        #     logger.warning("路径偏差太远!")
        #     return True
        # if self.is_on_ground and self.steps > 100:  # 地面超过 100 步未起飞
        #     logger.warning("起飞超时!")
        #     return True
        if not self.is_on_ground and self.current_waypoint_idx >= len(self.waypoints):
            logger.info("All waypoints reached!")
            self._plot_trajectory()
            return True

        return False

